/*
 * MecanumDriveSubsystem.cpp
 *
 *  Created on: Sep 24, 2011
 *      Author: David
 */

// Include files.
#include <cmath>
#include <boost/smart_ptr/shared_ptr.hpp>
#include "../Internal/FpkException.hpp"
#include "../Internal/FpkAssert.hpp"
#include "MecanumDriveSubsystem.hpp"
#include "../Filters/Drive/TankStrafeDriveFilter.hpp"

// Use namespaces.
using namespace boost;

// Define motor position rank constants.
static const UINT32 Front = 0;
static const UINT32 Back = 1;

MecanumDriveSubsystem::DriveDistance::DriveDistance()
{
	m_driveMode = DriveModeNone;
	m_x = 0;
	m_y = 0;
	m_rotation = 0;
	m_threshold = 0;
}

MecanumDriveSubsystem::DriveDistance::DriveDistance
(
	DriveMode driveMode,
	double x,
	double y,
	double rotation,
	double threshold
)
{
	m_driveMode = driveMode;
	m_x = x;
	m_y = y;
	m_rotation = rotation;
	m_threshold = threshold;
}

double MecanumDriveSubsystem::DriveDistance::GetWheelDistance(const MecanumDriveSubsystem& drive, MotorPosition position)
{
	/* The following code in the block after
	 * the next block is derived from WPILib, which 
	 * is licenced under the licence in the file 
	 * "Licence-WPILib.txt" inside the project directory.
	 */
	
	if (m_driveMode == DriveModeNone)
		return 0;
	else if (m_driveMode == DriveModeCartesian)
	{
		double invertation;
		double x;
		double y;
		double rotation;
		
		invertation = drive.GetMotorInvertationMultiplier(position);
		y = m_y;
		
		if (position.GetSide() == Left)
		{
			// Left side.
			
			rotation = m_rotation;
			
			if (position.GetRank() == 0)
			{
				// Front wheel.
				x = m_x;
			}
			else
			{
				// Back wheel.
				x = -m_x;
			}
		}
		else
		{
			// Right side.
			
			rotation = -m_rotation;
			
			if (position.GetRank() == 0)
			{
				// Front wheel.
				x = -m_x;
			}
			else
			{
				// Back wheel.
				x = m_x;
			}
		}
		
		return (x + y + rotation) * invertation;
	}
	else if (m_driveMode == DriveModePolar)
	{
		// TODO: Support Polar.
		
		throw FpkException("Not Implemented");
	}
	else
		throw FpkException("Invalid Drive Mode");
}

MecanumDriveSubsystem::ScaleModifier::ScaleModifier(double scale) :
	m_scale(scale)
{
}

void MecanumDriveSubsystem::ScaleModifier::Modify
(
	CartesianValue &value
)
{
	CartesianValue old = value;
	
	value = CartesianValue
	(
		old.GetX() * m_scale,
		old.GetY() * m_scale,
		old.GetRotation() * m_scale
	);
}

MecanumDriveSubsystem::DeadZoneModifier::DeadZoneModifier(double minimumAbsValue) :
	m_minimumAbsValue(minimumAbsValue)
{
}

void MecanumDriveSubsystem::DeadZoneModifier::Modify
(
	CartesianValue &value
)
{
	CartesianValue result = value;
	
	//printf("In: X:%f, Y:%f, Z%f", value.m_x, value.m_y, value.m_rotation);
	// X Deadzone
	if (fabs(value.GetX()) < m_minimumAbsValue)
		result = CartesianValue(0, result.GetY(), result.GetRotation());
	else if(value.GetX() >= m_minimumAbsValue)
		result = CartesianValue((1/(1 - m_minimumAbsValue)) * (value.GetX() - m_minimumAbsValue), result.GetY(), result.GetRotation());
	else
		result = CartesianValue((1/(1 - m_minimumAbsValue)) * (value.GetX() + m_minimumAbsValue), result.GetY(), result.GetRotation());
	
	// Y Deadzone
	if (fabs(value.GetY()) < m_minimumAbsValue)
		result = CartesianValue(result.GetX(), 0, result.GetRotation());
	else if(value.GetY() >= m_minimumAbsValue)
		result = CartesianValue(result.GetX(), (1/(1 - m_minimumAbsValue)) * (value.GetY() - m_minimumAbsValue), result.GetRotation());
	else
		result = CartesianValue(result.GetX(), (1/(1 - m_minimumAbsValue)) * (value.GetY() + m_minimumAbsValue), result.GetRotation());
	
	// Rotation Deadzone
	if (fabs(value.GetRotation()) < m_minimumAbsValue)
		result = CartesianValue(result.GetX(), result.GetY(), 0);
	else if(value.GetRotation() >= m_minimumAbsValue)
		result = CartesianValue(result.GetX(), result.GetY(), (1/(1 - m_minimumAbsValue)) * (value.GetRotation() - m_minimumAbsValue));
	else
		result = CartesianValue(result.GetX(), result.GetY(), (1/(1 - m_minimumAbsValue)) * (value.GetRotation() + m_minimumAbsValue));
	
	value = result;
	//printf(" Out: X:%f, Y:%f, Z%f\n", value.m_x, value.m_y, value.m_rotation);
}

MecanumDriveSubsystem::DriveStartState::DriveStartState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation
) :
	m_drive(drive),
	m_distance(mode, x, y, rotation, drive.GetDistanceThreshold())
{
}

MecanumDriveSubsystem::DriveStartState::DriveStartState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation,
	double threshold
) :
	m_drive(RequireControl(drive)),
	m_distance(mode, x, y, rotation, threshold)
{
}

void MecanumDriveSubsystem::DriveStartState::HandleActivate()
{
	switch (m_distance.m_driveMode)
	{
		/*
		case DriveModeCartesian:
			SetAsyncResult
			(
				m_drive.DriveDistanceByCartesian
				(
					m_distance.m_x,
					m_distance.m_y,
					m_distance.m_rotation
				)
			);
			break;
		case DriveModePolar:
			SetAsyncResult
			(
				m_drive.DriveDistanceByPolar
				(
					m_distance.m_x,
					m_distance.m_y,
					m_distance.m_rotation
				)
			);
			break;
		*/
		default:
			// TODO: Stop copying and pasting exception text.
			throw FpkException("Drive States Broken and/or Invalid Drive Mode");
			break;
	}
}

MecanumDriveSubsystem::DriveState::DriveState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation
) :
	AsyncResultEffectState<bool>(),
	m_stateStart(drive, mode, x, y, rotation)
{
	SetStartState(m_stateStart);
	AddResultDoneCodeMapping(true, StateDoneCode::GetSuccessCode());
}

MecanumDriveSubsystem::DriveState::DriveState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation,
	double threshold
) :
	AsyncResultEffectState<bool>(),
	m_stateStart(drive, mode, x, y, rotation, threshold)
{
	SetStartState(m_stateStart);
	AddResultDoneCodeMapping(true, StateDoneCode::GetSuccessCode());
}

CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	GenericHID &leftJoystick, 
	GenericHID &rightJoystick
)
{
	return TankStrafeDriveFilter().Apply(TwoJoystickValue(leftJoystick, rightJoystick));
}

CartesianValue MecanumDriveSubsystem::TankDriveWithActiveMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	CartesianValue value;
	
	if (leftJoystick.GetTrigger() && !rightJoystick.GetTrigger())
	{
		value = CartesianValue
		(
			leftJoystick.GetX(),
			-leftJoystick.GetY(),
			0
		);
	}
	else if (!leftJoystick.GetTrigger() && rightJoystick.GetTrigger())
	{
		value = CartesianValue
		(
			rightJoystick.GetX(),
			-rightJoystick.GetY(),
			0
		);
	}
	else if (leftJoystick.GetTrigger() && rightJoystick.GetTrigger())
	{
		value = CartesianValue
		(
			0,
			-((leftJoystick.GetY() + rightJoystick.GetY()) / 2),
			(rightJoystick.GetY() - leftJoystick.GetY()) / 2
		);
	}
	else
	{
		value = CartesianValue(0, 0, 0);
	}
	
	//printf("Drive Mode: Two Joysticks, Tank Passive \n");
	
	return value;
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	return ArcadeStrafeDriveFilter().Apply(TwoJoystickValue(leftJoystick, rightJoystick));
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	return ArcadeModernDriveFilter().Apply(TwoJoystickValue(leftJoystick, rightJoystick));
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	return ArcadeModernDriveFilter().Apply(TwoJoystickValue(joystick));
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	return ArcadeStrafeDriveFilter().Apply(TwoJoystickValue(joystick));
}

CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	return TankStrafeDriveFilter().Apply(TwoJoystickValue(joystick));
}

CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	CartesianValue value = CartesianValue
	(
			(joystick.GetAxisValue(Ps3Joystick::kRightX) + joystick.GetAxisValue(Ps3Joystick::kLeftX)) / 2,
			-(joystick.GetAxisValue(Ps3Joystick::kRightY) + joystick.GetAxisValue(Ps3Joystick::kLeftY)) / 2,
			(joystick.GetAxisValue(Ps3Joystick::kRightY) - joystick.GetAxisValue(Ps3Joystick::kLeftY)) / 2
	);
	
	//printf("Drive Mode: PS3, Tank Passive \n");
	
	return value;
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	CartesianValue value = CartesianValue
	(
		joystick.GetAxisValue(Ps3Joystick::kLeftX),
		-joystick.GetAxisValue(Ps3Joystick::kLeftY),
		joystick.GetAxisValue(Ps3Joystick::kRightX)
	);
	
	//printf("Drive Mode: PS3, Arcade Modern \n");
	
	return value;
}

CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	CartesianValue value = CartesianValue
	(
		joystick.GetAxisValue(Ps3Joystick::kLeftX),
		joystick.GetAxisValue(Ps3Joystick::kRightY),
		joystick.GetAxisValue(Ps3Joystick::kRightX)
	);
	
	//printf("Drive Mode: PS3, Arcade Normal \n");
	
	return value;
}

CartesianValue MecanumDriveSubsystem::TiltDriveStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	CartesianValue value = CartesianValue
	(
		-joystick.GetAxisValue(Ps3Joystick::kLeftRightTilt),
		-joystick.GetAxisValue(Ps3Joystick::kFrontBackTilt),
		joystick.GetAxisValue(Ps3Joystick::kRightX)
	);
	
	//printf("%f, %f, %f \n", value.m_x, value.m_y, value.m_rotation);
	//printf("Drive Mode: PS3, Tilt Style \n");
	
	return value;
}

void MecanumDriveSubsystem::DriveByCartesian
(
	MecanumDriveSubsystem &drive, 
	const CartesianValue &value
)
{
	drive.DriveByCartesian(value.GetX(), value.GetY(), value.GetRotation());
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel, 
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel
) :
	TankDriveSubsystem
	(
		leftFrontMotorChannel,
		leftBackMotorChannel,
		rightFrontMotorChannel,
		rightBackMotorChannel
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel, 
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel,
	Gyro *gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotorChannel,
		leftBackMotorChannel,
		rightFrontMotorChannel,
		rightBackMotorChannel
	)
	, m_gyro(gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel, 
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel,
	Gyro &gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotorChannel,
		leftBackMotorChannel,
		rightFrontMotorChannel,
		rightBackMotorChannel
	)
	, m_gyro(&gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	shared_ptr<PidMotorController> leftFrontMotor, 
	shared_ptr<PidMotorController> leftBackMotor,
	shared_ptr<PidMotorController> rightFrontMotor, 
	shared_ptr<PidMotorController> rightBackMotor
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	shared_ptr<PidMotorController> leftFrontMotor, 
	shared_ptr<PidMotorController> leftBackMotor,
	shared_ptr<PidMotorController> rightFrontMotor, 
	shared_ptr<PidMotorController> rightBackMotor,
	Gyro *gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	shared_ptr<PidMotorController> leftFrontMotor, 
	shared_ptr<PidMotorController> leftBackMotor,
	shared_ptr<PidMotorController> rightFrontMotor, 
	shared_ptr<PidMotorController> rightBackMotor,
	Gyro &gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(&gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	PidMotorController *leftFrontMotor, 
	PidMotorController *leftBackMotor,
	PidMotorController *rightFrontMotor, 
	PidMotorController *rightBackMotor,
	bool canDeleteMotors
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor,
		canDeleteMotors
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	PidMotorController *leftFrontMotor, 
	PidMotorController *leftBackMotor,
	PidMotorController *rightFrontMotor, 
	PidMotorController *rightBackMotor,
	Gyro *gyro,
	bool canDeleteMotors
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor,
		canDeleteMotors
	)
	, m_gyro(gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	PidMotorController &leftFrontMotor, 
	PidMotorController &leftBackMotor,
	PidMotorController &rightFrontMotor, 
	PidMotorController &rightBackMotor
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	PidMotorController &leftFrontMotor, 
	PidMotorController &leftBackMotor,
	PidMotorController &rightFrontMotor, 
	PidMotorController &rightBackMotor,
	Gyro &gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(&gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor,
	bool canDeleteMotors
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor,
		canDeleteMotors
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor,
	Gyro *gyro,
	bool canDeleteMotors
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor,
		canDeleteMotors
	)
	, m_gyro(gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(NULL)
{
	Startup();
}

MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor,
	Gyro &gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(&gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController *leftFrontMotor, 
	SpeedController *leftBackMotor,
	SpeedController *rightFrontMotor, 
	SpeedController *rightBackMotor
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController *leftFrontMotor, 
	SpeedController *leftBackMotor,
	SpeedController *rightFrontMotor, 
	SpeedController *rightBackMotor,
	Gyro *gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(gyro)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController &leftFrontMotor, 
	SpeedController &leftBackMotor,
	SpeedController &rightFrontMotor, 
	SpeedController &rightBackMotor
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(NULL)
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController &leftFrontMotor, 
	SpeedController &leftBackMotor,
	SpeedController &rightFrontMotor, 
	SpeedController &rightBackMotor,
	Gyro &gyro
) :
	TankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(&gyro)
{
	Startup();
}

/**
 *  Destructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::~MecanumDriveSubsystem()
{
	delete m_safetyHelper;
}

/**
 *  Drive the specified amount on a cartesian plane.
 *  
 *  Testing shows this function works.
 *  
 *  @param x The amount to drive on the x-axis.
 *  @prarm y The amount to drive on the y-axis.
 *  @prarm rotation The amount to rotate.
 */
void MecanumDriveSubsystem::DriveByCartesian(double x, double y, double rotation)
{
	Action action(this);
	
	Awake();
	CancelAllOperations();
	
	// Apply reversal if needed.
	if (m_reversed)
	{
		x *= -1;
		y *= -1;
	}
	
	rotation = CompensateWithGyro(CartesianValue(x, y, rotation)).GetRotation();
	
	// Use "-y" to negate RobotDrive's negation
	m_drive.MecanumDrive_Cartesian(x, -y, rotation);
	FeedSafety();
	//printf("actual motor speed : %f ", m_cjLeftBack.Get());
	//printf("requested motor speed : %f \n", (-x + y + rotation) * GetMotorInvertationMultiplier(m_motorInvertationRightBack) * m_maxOutput);
}

/**
 *  Drive the specified amount using polar coordinates.
 *  
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to drive.
 *  @prarm direction The direction to drive.
 *  @prarm rotation The amount to rotate.
 */
void MecanumDriveSubsystem::DriveByPolar(double magnitude, double direction, double rotation)
{
	Action action(this);
	
	Awake();
	CancelAllOperations();
	
	// TODO: Support reversals for DriveByPolar.
	// TODO: Use gyro to compensate.
	
	m_drive.MecanumDrive_Polar(magnitude, direction, rotation);
	FeedSafety();
}

/**
 *  Drive a distance specified by values in cartesian form.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 * 
 *  This function doesn't seem to work.
 *  
 *  @param x Distance to move on the X axis reletive to the robot.
 *  @param y Distance to move on the Y axis reletive to the robot.
 *  @param rotation Amount to rotate.
 *  
 *  @returns A shared pointer to an ayncronous result of type bool.
 */
shared_ptr<Operation<bool> > MecanumDriveSubsystem::DriveDistanceByCartesian(double x, double y, double rotation)
{
	return DriveDistanceByCartesian(x, y, rotation, GetDistanceThreshold());
}

shared_ptr<Operation<bool> > MecanumDriveSubsystem::DriveDistanceByCartesian(double x, double y, double rotation, double threshold)
{
	return DriveDistanceByCartesian(CartesianValue(x, y, rotation), threshold);
}

shared_ptr<Operation<bool> > MecanumDriveSubsystem::DriveDistanceByCartesian(const CartesianValue &value, double threshold)
{
	Action action(this);
	
	Awake();
	CancelAllOperations();
	return Launch(new DriveDistanceOperation(*this, value, threshold), true);
}

/**
 *  Drive a distance specified by values in polar form.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 * 
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to move.
 *  @param direction The direction to move.
 *  @param rotation The amount to rotate.
 *  
 *  @returns A shared pointer to an ayncronous result of type bool.
 */
shared_ptr<Operation<bool> > MecanumDriveSubsystem::DriveDistanceByPolar(double magnitude, double direction, double rotation)
{
	Action action(this);
	
	// TODO: Support Polar.
	
	Awake();
	throw FpkException("Polar not yet supported.");
}

void MecanumDriveSubsystem::HandlePeriodic()
{
	Action action(this);
	
	TankDriveSubsystem::HandlePeriodic();
}

CartesianValue MecanumDriveSubsystem::CompensateWithGyro(CartesianValue value)
{
	static const double c_coefficient = 1/20; 
	
	if (m_gyro != NULL)
	{
		if (value.GetRotation() == 0)
			m_gyroAnglePrevious = m_gyro->GetAngle();
		else
		{
			value = CartesianValue(value.GetX(), value.GetY(), (m_gyroAnglePrevious - m_gyro->GetAngle())*c_coefficient);
		}
	}
	
	return value;
}

void MecanumDriveSubsystem::Startup()
{
	m_gyroAnglePrevious = 0;
}

MecanumDriveSubsystem::DriveDistanceOperation::DriveDistanceOperation(MecanumDriveSubsystem &drive, const CartesianValue &value, double threshold) :
	m_drive(drive),
	m_value(value),
	m_threshold(threshold)
{
}

void MecanumDriveSubsystem::DriveDistanceOperation::HandleLaunch()
{
	m_controlModePrevious = m_drive.GetControlMode();
	m_drive.SetControlMode(ControlModeDistance);
}

void MecanumDriveSubsystem::DriveDistanceOperation::HandlePeriodic()
{
	UINT8 syncGroup = 0x80;
	
	double x = m_value.GetX();
	double y = m_value.GetY();
	double rotation = m_value.GetRotation();
	
	// TODO: Use gyro to compensate.
	
	double leftFront = (x + y + rotation) * m_drive.GetMotorInvertationMultiplier(Left, Front);
	double rightFront = (-x + y - rotation) * m_drive.GetMotorInvertationMultiplier(Right, Front);
	double leftBack = (-x + y + rotation) * m_drive.GetMotorInvertationMultiplier(Left, Back);
	double rightBack = (x + y - rotation) * m_drive.GetMotorInvertationMultiplier(Right, Back); 
	
	m_drive.GetPidMotorController(Left, Front).Set(leftFront, syncGroup);
	m_drive.GetPidMotorController(Right, Front).Set(rightFront, syncGroup);
	m_drive.GetPidMotorController(Left, Back).Set(leftBack, syncGroup);
	m_drive.GetPidMotorController(Right, Back).Set(rightBack, syncGroup);
	
	CANJaguar::UpdateSyncGroup(syncGroup);
	
	bool finished = true;
	double positionLeftFront = m_drive.GetPidMotorController(Left, Front).GetPosition();
	double positionRightFront = m_drive.GetPidMotorController(Right, Front).GetPosition();
	double positionLeftBack = m_drive.GetPidMotorController(Left, Back).GetPosition();
	double positionRightBack = m_drive.GetPidMotorController(Right, Back).GetPosition();
	
	finished = finished && (abs(positionLeftFront - leftFront) < m_threshold);
	finished = finished && (abs(positionRightFront - rightFront) < m_threshold);
	finished = finished && (abs(positionLeftBack - leftBack) < m_threshold);
	finished = finished && (abs(positionRightBack - rightBack) < m_threshold);
	
	m_drive.FeedSafety();
	
	if (finished)
	{
		m_drive.SetControlMode(m_controlModePrevious);
		Finish(true);
	}
}

bool MecanumDriveSubsystem::DriveDistanceOperation::HandleCancel()
{
	m_drive.SetControlMode(m_controlModePrevious);
	return false;
}
